import sys

import rospy
from moveit_commander import MoveGroupCommander, roscpp_initialize
import moveit_commander
from geometry_msgs.msg import PoseStamped
from moveit_commander import PlanningSceneInterface
from rospy.rostime import get_time
# remap again

class move_arm():
    def __init__(self):
        joint_state_topic = ['joint_states:=/j2n6s300/joint_states']
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('foo', anonymous=False)
        roscpp_initialize(sys.argv)

        self.arm = MoveGroupCommander('arm')
        self.arm.set_max_acceleration_scaling_factor(1)
        self.arm.set_max_velocity_scaling_factor(1)
        self.arm.set_pose_reference_frame("root")
        self.end_effector_link = self.arm.get_end_effector_link()
        self.arm.set_start_state_to_current_state()
        scene = PlanningSceneInterface()


        self.gripper = MoveGroupCommander('gripper')
        self.gripper.set_max_acceleration_scaling_factor(1)
        self.gripper.set_max_velocity_scaling_factor(1)
        

    def getpose(self,x,y,z):
        target_pose = PoseStamped()
        target_pose.header.frame_id = "root"
        target_pose.header.stamp = rospy.Time.now()
        target_pose.pose.position.x = x
        target_pose.pose.position.y = y
        target_pose.pose.position.z = z
        target_pose.pose.orientation.x = 0
        target_pose.pose.orientation.y = 0
        target_pose.pose.orientation.z = 0
        target_pose.pose.orientation.w = 1
        return target_pose

    def close_gripper(self):
        for _ in range(5):
            self.gripper.set_joint_value_target([1.1,1.1,1.1])
            self.gripper.go()
        print("Gripper Closed")
    def open_gripper(self):
        self.gripper.set_joint_value_target([0.1,0.1,0.1])
        self.gripper.go()
        print("Gripper Openned")


    def move_arm(self,x,y,z=0.3):
        target_pose =self.getpose(x,y,z)
        self.arm.set_pose_target(target_pose, "j2n6s300_link_6")
        self.arm.go()
    def grasp_object(self,x,y,z):
        self.open_gripper()
        self.move_arm(x,y,z+0.1)
        self.move_arm(x,y,z)
        self.close_gripper()
        self.move_arm(x,y,z+0.2)
        self.move_arm(0.3,0.3,0.3)
        self.move_arm(0.3,0.3,0.25)
        self.open_gripper()

if __name__ == '__main__':
    x = move_arm()
    for _ in range(10):
        x.grasp_object(0.2,-0.3,0.22)